
#include <rtdbg.h>
#include <drivers/i2c.h>
#include <drv_soft_i2c.h>
#include <drivers/pin.h>
#include "QMC5883_sensor.h"
#include "sensor/read_sensor.h"

#define DBG_ENABLE

/* */

/// @brief 根据32bit的range映射到枚举类型QMC5883_Range
static enum QMC5883_Range find_qmc5883_range(rt_uint32_t rt_range);

/// @brief 根据32bit的range映射到枚举类型QMC5883_odr
static enum QMC5883_odr find_qmc5883_odr(rt_uint16_t rt_odr);
/* */

/// @brief 该函数向QMC5883驱动提供延时接口
/// @param period 延时ms时长
/// @return
static int8_t qmc5883_delay_ms(uint32_t period);

/// @brief 该函数向QMC5883驱动提供以I2C方式读取寄存器接口
/// @param reg 寄存器地址
/// @param data_buf 保存读取寄存器的数据
/// @param data_len 指定读取长度
/// @return 读取成功RT_EOK ,失败 RT_EFAIL
static int8_t qmc5883_i2c_read_reg(uint8_t reg, uint8_t *data_buf, uint16_t data_len);

/// @brief 该函数向QMC5883驱动提供以I2C方式写入寄存器接口
/// @param reg 寄存器地址
/// @param data_buf 保存写入寄存器的数据
/// @param data_len 指定写入长度
/// @return 写入成功或者失败
static int8_t qmc5883_i2c_write_reg(uint8_t reg, uint8_t *data_buf, uint16_t data_len);

/// @brief 当使用I2C协议时创建QMC5883设备
/// @param intf
/// @return
static qmc5883_dev *qmc5883_create_i2c(struct rt_sensor_intf *intf);

/// @brief 向RT-Thread提供控制QMC5883接口函数，根据不同命令使bmi270做出不同反应
/// @param sensor QMC5883设备
/// @param cmd 命令字
/// @param args 参数
/// @return
static rt_err_t qmc5883_control(struct rt_sensor_device *sensor, int cmd, void *args);

/// @brief 向RT-Thread提供读取QMC5883传感数据接口函数
/// @param sensor QMC5883设备
/// @param buf 存储地址
/// @param len 读取长度
/// @return
static rt_size_t qmc5883_fetch_data(struct rt_sensor_device *sensor, void *buf, rt_size_t len);

/// @brief QMC5883向RT-Thread注册地磁传感器设备
/// @param cfg
/// @param hdev
/// @return
static int8_t qmc5883_sensor_register(struct rt_sensor_config *cfg, void *hdev);

/// @brief 开始启动QMC5883
/// @param
/// @return
static int rt_start_qmc5883_app(void);

static qmc5883_dev *sg_qmc5883_dev = RT_NULL;

static int rt_start_qmc5883_app(void)
{
    struct rt_sensor_config cfg;
    cfg.intf.dev_name = QMC5883_I2C_BUSNAME;
    cfg.intf.user_data = (void *)(0x69);
    cfg.intf.type = RT_SENSOR_INTF_I2C;

    return rt_hw_qmc5883_init(&cfg);
}
INIT_APP_EXPORT(rt_start_qmc5883_app);

int8_t rt_hw_qmc5883_init(struct rt_sensor_config *cfg)
{
    int8_t rslt = RT_EOK;
    qmc5883_dev *hdev = RT_NULL;

    /* step1 : create QMC5883 device */
    hdev = qmc5883_create_i2c(&cfg->intf);
    if (hdev == RT_NULL)
    {
        rt_kprintf("\n qmc5883 sensor create failed\n");
        return RT_ERROR;
    }
    rt_kprintf("\n qmc5883 sensor create success\n");

    if (qmc5883_init(hdev) != QMC5883_OK)
    {
        rt_kprintf("\n QMC5883 init failed, chip id = %d\n", hdev->chip_id);
        return RT_ERROR;
    }
    rt_kprintf("\n QMC5883 init success\n");
    sg_qmc5883_dev = hdev;

    /* step2 : regist QMC5883 sensor into RT-Thread sensor driver */
    rslt = qmc5883_sensor_register(cfg, hdev);
    if (rslt != RT_EOK)
    {
        rt_kprintf("\n qmc5883 sensor register failed\n");
        return RT_ERROR;
    }
    rt_kprintf("\n qmc5883 sensor create and register success\n");

    return RT_EOK;
}

static enum QMC5883_Range find_qmc5883_range(rt_uint32_t rt_range)
{
    enum QMC5883_Range range = range_30_Gauss;

    if (rt_range <= 2000)
    {
        range = range_2_Gauss;
    }
    else if (rt_range <= 8000)
    {
        range = range_8_Gauss;
    }
    else if (rt_range <= 12000)
    {
        range = range_12_Gauss;
    }
    else
    {
        range = range_30_Gauss;
    }

    return range;
}

static enum QMC5883_odr find_qmc5883_odr(rt_uint16_t rt_odr)
{
    enum QMC5883_odr odr = ODR_200Hz;

    if (odr <= 10)
    {
        odr = ODR_10Hz;
    }
    else if (odr <= 50)
    {
        odr = ODR_50Hz;
    }
    else if (odr <= 100)
    {
        odr = ODR_100Hz;
    }
    else
    {
        odr = ODR_200Hz;
    }

    return odr;
}

static int8_t qmc5883_delay_ms(uint32_t period)
{
    return rt_thread_mdelay(period);
}

static int8_t qmc5883_i2c_read_reg(uint8_t reg, uint8_t *data_buf, uint16_t data_len)
{
    struct rt_i2c_bus_device *bus = rt_i2c_bus_device_find("i2c2");
    if (bus == RT_NULL)
    {
        rt_kprintf("error, i2c2 bus not exist\n");
        return RT_ERROR;
    }

    rt_uint8_t tmp = reg;
    struct rt_i2c_msg msgs[2];

    msgs[0].addr = QMC5880L_I2C_ADDR; /* Slave address */
    msgs[0].flags = RT_I2C_WR;        /* Write flag */
    msgs[0].buf = &tmp;               /* Slave register address */
    msgs[0].len = 1;                  /* Number of bytes sent */

    msgs[1].addr = QMC5880L_I2C_ADDR; /* Slave address */
    msgs[1].flags = RT_I2C_RD;        /* Read flag */
    msgs[1].buf = data_buf;           /* Read data pointer */
    msgs[1].len = data_len;           /* Number of bytes read */

    if (rt_i2c_transfer(bus, msgs, 2) != 2)
    {
        return RT_ERROR;
    }

    return RT_EOK;
}

static int8_t qmc5883_i2c_write_reg(uint8_t reg, uint8_t *data_buf, uint16_t data_len)
{
    struct rt_i2c_bus_device *bus = rt_i2c_bus_device_find("i2c2");
    if (bus == RT_NULL)
    {
        rt_kprintf("error, bus i2c2 not exist\n");
        return RT_ERROR;
    }

    rt_uint8_t tmp = reg;
    struct rt_i2c_msg msgs[2];

    msgs[0].addr = QMC5880L_I2C_ADDR; /* Slave address */
    msgs[0].flags = RT_I2C_WR;        /* Write flag */
    msgs[0].buf = &tmp;               /* Slave register address */
    msgs[0].len = 1;                  /* Number of bytes sent */

    msgs[1].addr = QMC5880L_I2C_ADDR;            /* Slave address */
    msgs[1].flags = RT_I2C_WR | RT_I2C_NO_START; /* Read flag */
    msgs[1].buf = data_buf;                      /* Read data pointer */
    msgs[1].len = data_len;                      /* Number of bytes read */

    if (rt_i2c_transfer(bus, msgs, 2) != 2)
    {
        return RT_ERROR;
    }

    return RT_EOK;
}

static qmc5883_dev *qmc5883_create_i2c(struct rt_sensor_intf *intf)
{
    struct rt_i2c_bus_device *i2c_bus_dev = RT_NULL;
    qmc5883_dev *hdev = RT_NULL;

    i2c_bus_dev = (struct rt_i2c_bus_device *)rt_i2c_bus_device_find(intf->dev_name);
    if (i2c_bus_dev == RT_NULL)
    {
        rt_kprintf("can not find device %s", intf->dev_name);
        return RT_NULL;
    }

    hdev = rt_calloc(1, sizeof(qmc5883_dev));
    if (hdev == RT_NULL)
    {
        rt_kprintf("bmi270 dev memory allocation failed");
        return RT_NULL;
    }

    hdev->read = qmc5883_i2c_read_reg;
    hdev->write = qmc5883_i2c_write_reg;
    hdev->delay_ms = qmc5883_delay_ms;

    return hdev;
}

static rt_err_t qmc5883_control(struct rt_sensor_device *sensor, int cmd, void *args)
{
    qmc5883_dev *hdev = sensor->parent.user_data;
    int8_t result = RT_EOK;
    enum QMC5883_Range range;
    enum QMC5883_odr odr;

    switch (cmd)
    {
    case RT_SENSOR_CTRL_GET_ID:
        *(rt_uint8_t *)args = hdev->chip_id;
        break;
    case RT_SENSOR_CTRL_SET_ODR:
        odr = find_qmc5883_odr((rt_uint32_t)args & 0xffff);
        result = qmc5883_select_odr(odr, hdev);
        break;
    case RT_SENSOR_CTRL_SET_RANGE:
        range = find_qmc5883_range((rt_uint32_t)args);
        result = qmc5883_select_range(range, hdev);
        break;
    case RT_SENSOR_CTRL_SET_POWER:
        result = qmc5883_select_pwr_mode((rt_uint32_t)args & 0xff, hdev);
        break;
    case RT_SENSOR_CTRL_SET_MODE:
        /* only polling mode */
        break;
    case RT_SENSOR_CTRL_SELF_TEST:
        result = qmc5883_self_test(hdev);
        break;
    default:
        return -RT_EINVAL;
    }
    return result;
}

/* 向RT-Thread驱动框架提供获取数据和控制命令的接口 */
static struct rt_sensor_ops qmc5883_sensor_ops = {qmc5883_fetch_data, qmc5883_control};

static rt_size_t qmc5883_fetch_data(struct rt_sensor_device *sensor, void *buf, rt_size_t len)
{
    qmc5883_mag_data_t mag_data;
    int8_t rslt = qmc5883_get_sensor_data(&mag_data, sg_qmc5883_dev);
    if (rslt != QMC5883_OK)
    {
        return 0;
    }

    struct rt_sensor_data *data = (struct rt_sensor_data *)buf;

    data->type = RT_SENSOR_CLASS_MAG;
    data->timestamp = rt_tick_get();
    data->data.mag.x = mag_data.mag_x.axis_adc;
    data->data.mag.y = mag_data.mag_y.axis_adc;
    data->data.mag.z = mag_data.mag_z.axis_adc;

    return sizeof(mag_data);
}

static int8_t qmc5883_sensor_register(struct rt_sensor_config *cfg, void *hdev)
{
    int8_t rslt = RT_EOK;
    rt_sensor_t sensor_mag = RT_NULL;

    sensor_mag = rt_calloc(1, sizeof(struct rt_sensor_device));
    if (sensor_mag == RT_NULL)
    {
        return RT_ERROR;
    }
    sensor_mag->info.type = RT_SENSOR_CLASS_MAG;
    sensor_mag->info.vendor = RT_SENSOR_VENDOR_UNKNOWN;
    sensor_mag->info.model = "qmc5883_mag";
    sensor_mag->info.unit = RT_SENSOR_UNIT_MGAUSS;
    sensor_mag->info.intf_type = RT_SENSOR_INTF_I2C;
    sensor_mag->info.range_max = QMC5883_MAG_MAX_RANGE;
    sensor_mag->info.range_min = QMC5883_MAG_MIN_RANGE;
    sensor_mag->info.period_min = QMC5883_MAG_MIN_PERIOD_MS_RT;
    sensor_mag->info.fifo_max = QMC5883_MAG_FIFO_BUF_MIN; /* no fifo */

    rt_memcpy(&sensor_mag->config, cfg, sizeof(struct rt_sensor_config));
    sensor_mag->ops = &qmc5883_sensor_ops;

    rslt = rt_hw_sensor_register(sensor_mag, QMC5883_DEVICE_NAME, RT_DEVICE_FLAG_RDWR, hdev);
    if (rslt != RT_EOK)
    {
        rt_kprintf("QMC5883 regist mag failed\n");
        return rslt;
    }
    rt_kprintf("QMC5883 regist mag success\n");

    return rslt;
}

static void qmc5883_sensor_sample(void)
{
    rt_device_t dev = RT_NULL;
    struct rt_sensor_data data;
    rt_size_t i;

    /* 查找系统中的传感器设备 */
    dev = rt_device_find("mag_QMC5883");
    if (dev == RT_NULL)
    {
        rt_kprintf("Can't find mag_QMC5883 device\n");
        return;
    }

    for (i = 0; i < 5; i++)
    {
        /* 从传感器读取一个数据 */
        rt_device_read(dev, 0, &data, 1);
        sensor_show_data((rt_sensor_t)dev, &data);
        rt_thread_mdelay(100);
    }
}
MSH_CMD_EXPORT(qmc5883_sensor_sample, qmc5883_device_sample);
